#include<SoftwareSerial.h>

#define LEDPIN 4
#define TRXRATE 28800
#define TERMBAUD 9600
#define RX_1 6
#define TX_1 7
#define RX_2 8
#define TX_2 9
#define RX_3 10
#define TX_3 11
#define RX_4 12
#define TX_4 13
SoftwareSerial mySerial_1(RX_1,TX_1);
SoftwareSerial mySerial_2(RX_2,TX_2);
SoftwareSerial mySerial_3(RX_3,TX_3);
SoftwareSerial mySerial_4(RX_4,TX_4);

int isFirst;
long rand100;
int collecting_IDs;
uint8_t IDs[16];
uint8_t ID;
int id_count;
int noNewID = 0;
char msg_buf[16] = "TEST_MESSAGE";

//https://stackoverflow.com/questions/15094834/check-if-a-value-exist-in-a-array
int isvalueinarray(byte val, byte *arr, int size){
    int i;
    for (i=0; i < size; i++) {
        if (arr[i] == val)
            return 1;
    }
    return 0;
}

//Write ID buffer to serial TX
void writebuffertoserial(int channel, uint8_t *arr, int buf_size) {
  int i;
  Serial.print("Writing");
  for(i=0;i<buf_size;i++){
    Serial.print(", ");
    Serial.print(arr[i], DEC);
  }
  Serial.print(" to serial channel ");
  Serial.println(channel, DEC);
  switch(channel){
    case 1:
      mySerial_1.write(arr, buf_size);
      break;
    case 2:
      mySerial_2.write(arr, buf_size);
      break;
    case 3:
      mySerial_3.write(arr, buf_size);
      break;
    case 4:
      mySerial_4.write(arr, buf_size);
      break;
    default:
      Serial.println("Invalid channel selection for writebuffertoserial()");
      Serial.print("Error! cannot write to serial channel ");
      Serial.println(channel);
      break;
    }
}

char readbufferserial(int channel, uint8_t *arr, int buf_size) {
  char serial_num;
  switch(channel) {
    case 1:
      Serial.println(mySerial_1.peek(), DEC);
      if(mySerial_1.available()){
        ID = mySerial_1.read();
        serial_num = '1';
      }
      break;
    case 2:
      Serial.println(mySerial_2.peek(), DEC);
      if(mySerial_2.available()){
        ID = mySerial_2.read();
        serial_num = '2';
      }
      break;
    case 3:
      Serial.println(mySerial_3.peek(), DEC);
      if(mySerial_3.available()){
        ID = mySerial_3.read();
        serial_num = '3';
      }
      break;
     case 4:
      Serial.println(mySerial_4.peek(), DEC);
      if(mySerial_4.available()){
        ID = mySerial_4.read();
        serial_num = '4';
      }
      break;
      default:
        Serial.println("Invalid channel selection for readbuffertoserial()");
        Serial.print("Error! cannot read serial channel ");
        Serial.println(channel);
        serial_num = '-1';
      break;
  }
  return serial_num;
}

void setup(){
  Serial.begin(TERMBAUD);
  Serial.println("attempting to initialize Serial comms...");
  Serial.println("...");
  pinMode(RX_1, INPUT);
  pinMode(TX_1, OUTPUT);
  pinMode(RX_2, INPUT);
  pinMode(TX_2, OUTPUT);
  pinMode(RX_3, INPUT);
  pinMode(TX_3, OUTPUT);
  pinMode(RX_4, INPUT);
  pinMode(TX_4, OUTPUT);
  mySerial_1.begin(TRXRATE);
  Serial.println("...");
  //delay(500);
  //mySerial_2.begin(TERMBAUD);
  //Serial.println("...");
  //mySerial_3.begin(TERMBAUD);
  //Serial.println("...");
  //mySerial_4.begin(TERNBAUD);
  pinMode(LEDPIN, OUTPUT);
  Serial.println("...initialization complete.");
  Serial.println("Generating random ID...");
  randomSeed(analogRead(A5));
  rand100 = random(100);
  IDs[0] = uint8_t(rand100);
  id_count = 1;
  collecting_IDs = 1;
  isFirst = 1;
  msg_buf[12] = 0;
  digitalWrite(LEDPIN, HIGH);
}

void loop(){
  if(collecting_IDs){
    writebuffertoserial(1, IDs, id_count);
    writebuffertoserial(2, IDs, id_count);
    writebuffertoserial(3, IDs, id_count);
    writebuffertoserial(4, IDs, id_count);
    int i;
    for(i = 1; i <= 4; ++i) {
      Serial.print("The next value in the buffer is ");
      char serial_num = readbufferserial(i, IDs, id_count);
      if(serial_num){
        Serial.print("Read ");
        Serial.print(ID, DEC);
        Serial.print(" from serial channel ");
        Serial.print(serial_num);
        Serial.println('.');
        if(ID > IDs[0] || isFirst != 1){
          digitalWrite(LEDPIN, LOW);
          isFirst = 0;
        }else{
          digitalWrite(LEDPIN, HIGH);
        }
        if(!isvalueinarray(ID, IDs, id_count)) {
          IDs[id_count] = ID;
          id_count++;
          Serial.print(ID, DEC);
          Serial.println(" added to the ID list.");
          Serial.print("id_count now equals: ");
          Serial.println(id_count, DEC);
          noNewID = 0;
        } else {
          noNewID++;
          if(noNewID > 25){
            Serial.print(id_count, DEC);
            Serial.println(" IDs found");
            collecting_IDs = 0;
          }
        }
      }
    }
  }
  if(isFirst == 1){
    digitalWrite(LEDPIN, HIGH);
  }else{
    writebuffertoserial(1, IDs, id_count);
    digitalWrite(LEDPIN, LOW);
  }
  /*digitalWrite(LEDPIN, HIGH);
  Serial.println("Pin is HIGH!");
  delay(2000);
  digitalWrite(LEDPIN, LOW);
  Serial.println("Pin is LOW!");
  delay(4000);*/
}
